#include "cvmanager.h"

#include <stdio.h>
#include <iostream>
#include"opencv2/opencv.hpp"
#include "opencv2/core/core.hpp"
#include "opencv2/features2d/features2d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/nonfree/nonfree.hpp"

using namespace controllers;
using namespace std;


CVManager::CVManager()
{
}

/**
 * @brief CVManager::iplImageToQImage
 * @param iplImage
 * @return
 * Adapted from:
 * http://josedolz.jimdo.com/computer-vision-tutorials/iplimage-to-qimage/
 */
QImage CVManager::iplImageToQImage(const IplImage *iplImage)
{
    int height = iplImage->height;
    int width = iplImage->width;

    const uchar *qImageBuffer =(const uchar*)iplImage->imageData;
    QImage img(qImageBuffer, width, height, QImage::Format_RGB888);

    return img.rgbSwapped();
}

/**
 * @brief CVManager::iplImageToQImage
 * @param iplImage
 * @param size
 * @return
 * Adapted from:
 * http://josedolz.jimdo.com/computer-vision-tutorials/iplimage-to-qimage/
 */
QImage CVManager::iplImageToQImage(const IplImage *iplImage, QSize size)
{
       int height = iplImage->height;
       int width = iplImage->width;

       const uchar *qImageBuffer = (const uchar*)iplImage->imageData;
       QImage img(qImageBuffer, width, height, QImage::Format_RGB888);
       img = img.scaled(size, Qt::KeepAspectRatio, Qt::FastTransformation);

       return img.rgbSwapped();
}

/**
 * @brief MatToQImage
 * @param matImage
 * @return
 */
QImage CVManager::matToQImage(const Mat &matImage)
{
    int width   = matImage.size().width;
    int height  = matImage.size().height;

    const uchar *qImageBuffer =(const uchar*)matImage.data;
    QImage img(qImageBuffer, width, height, QImage::Format_RGB888);

    return img.rgbSwapped();
}

/**
 * @brief MatToQImage
 * @param matImage
 * @param size
 * @return
 */
QImage CVManager::matToQImage(const Mat &matImage, QSize size)
{
    int width   = matImage.size().width;
    int height  = matImage.size().height;

    const uchar *qImageBuffer = (const uchar*)matImage.data;
    QImage img(qImageBuffer, width, height, QImage::Format_RGB888);
    img = img.scaled(size, Qt::KeepAspectRatio, Qt::FastTransformation);

    return img.rgbSwapped();
}


/**
 * @brief MatToQImage
 * @param matImage
 * @param size
 * @return
 */
QImage CVManager::matToQImage(const Mat &matImage, QSize size, int gray)
{
    int width   = matImage.size().width;
    int height  = matImage.size().height;
    static QVector<QRgb>  sColorTable;

    // only create our color table once
    if ( sColorTable.isEmpty() )
    {
       for ( int i = 0; i < 256; ++i )
          sColorTable.push_back( qRgb( i, i, i ) );
    }

    const uchar *qImageBuffer = (const uchar*)matImage.data;
    QImage img(qImageBuffer, width, height, QImage::Format_Indexed8); // One channel
    img = img.scaled(size, Qt::KeepAspectRatio, Qt::FastTransformation);

    img.setColorTable(sColorTable);

    return img;//.rgbSwapped();
}

/**
 * @brief qImageMatTo
 * @param qImage
 * @return
 */
Mat qImageMatTo(const QImage &qImage)
{
    // qImage must have three channels: i.e. RGB888 format
    //return Mat(qImage.rows(), qImage.cols(), CV_8UC3, qImage.scanline());
    return Mat();
}

void CVManager::sharpen2D(const Mat &image, Mat &result)
{
    // Construct kernel (all entries initialized to 0)
    Mat kernel(3, 3, CV_32F, Scalar(0));

    // assigns kernel values
    kernel.at<float>(1,1)= 5.0;
    kernel.at<float>(0,1)= -1.0;
    kernel.at<float>(2,1)= -1.0;
    kernel.at<float>(1,0)= -1.0;
    kernel.at<float>(1,2)= -1.0;

    //filter the image
    filter2D(image, result, image.depth(), kernel);
}

void CVManager::computeCanny(Mat& img, Mat& out)
{
    // Convert to gray
    //if (img.channels() == 3)
    //    cvtColor(img, out, CV_BGR2GRAY); // Problem

    // Compute Canny edges
    //Canny(img, out, 100, 200); // Problem

    // Invert the image
    threshold(img, out, 128, 255, THRESH_BINARY_INV);
}

void CVManager::computeLaplacian(const cv::Mat& image, Mat& result, int aperture, double scale)
{
    Mat laplace, copyImage;

    // Compute Laplacian
    Laplacian(image, laplace, CV_32F, aperture);

    if (scale < 0) {
        double lapmin, lapmax;
        minMaxLoc(laplace, &lapmin, &lapmax);
        scale = 127 / max(-lapmin, lapmax);
    }

    laplace.convertTo(result, CV_8U, scale, 128);
}

void CVManager::watermarkImage(Mat &matImage, Mat& watermarkImg, int x, int y)
{
    Mat imageROI; // Region of interest in matImage

    imageROI = matImage(cv::Rect(x, y, watermarkImg.cols, watermarkImg.rows));

    // add logo to image
    addWeighted(imageROI, 0.5, watermarkImg, 1.0, 0., imageROI);
}

Mat CVManager::findFeatures(Mat image1, Mat image2)
{
    Mat img_object;
    Mat img_scene;
    cv::cvtColor(image1, img_object, CV_BGR2GRAY);
    cv::cvtColor(image2, img_scene, CV_BGR2GRAY);
    //cv::cvtColor(image1, img_object, CV_RGB2GRAY);
    //cv::cvtColor(image2, img_scene, CV_RGB2GRAY);

    if( !img_object.data || !img_scene.data )
    { std::cout<< " --(!) Error reading images " << std::endl; return Mat(); }

    //-- Step 1: Detect the keypoints using SURF Detector
    int minHessian = 400;

    SurfFeatureDetector detector( minHessian );

    std::vector<KeyPoint> keypoints_object, keypoints_scene;

    detector.detect( img_object, keypoints_object );
    detector.detect( img_scene, keypoints_scene );

    //-- Step 2: Calculate descriptors (feature vectors)
    SurfDescriptorExtractor extractor;

    Mat descriptors_object, descriptors_scene;

    extractor.compute( img_object, keypoints_object, descriptors_object );
    extractor.compute( img_scene, keypoints_scene, descriptors_scene );

    //-- Step 3: Matching descriptor vectors using FLANN matcher
    FlannBasedMatcher matcher;
    std::vector< DMatch > matches;
    matcher.match( descriptors_object, descriptors_scene, matches );

    double max_dist = 0; double min_dist = 100;

    //-- Quick calculation of max and min distances between keypoints
    for( int i = 0; i < descriptors_object.rows; i++ )
    { double dist = matches[i].distance;
    if( dist < min_dist ) min_dist = dist;
    if( dist > max_dist ) max_dist = dist;
    }

    std::cout << "-- Max dist : " << max_dist << endl;
    std::cout << "-- Min dist : " << min_dist << endl;

    //-- Draw only "good" matches (i.e. whose distance is less than 3*min_dist )
    std::vector< DMatch > good_matches;

    for( int i = 0; i < descriptors_object.rows; i++ )
    { if( matches[i].distance < 3*min_dist )
     { good_matches.push_back( matches[i]); }
    }

    Mat img_matches;
    drawMatches( img_object, keypoints_object, img_scene, keypoints_scene,
               good_matches, img_matches, Scalar::all(-1), Scalar::all(-1),
               vector<char>(), DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );

    //-- Localize the object
    std::vector<Point2f> obj;
    std::vector<Point2f> scene;

    for( int i = 0; i < good_matches.size(); i++ )
    {
    //-- Get the keypoints from the good matches
    obj.push_back( keypoints_object[ good_matches[i].queryIdx ].pt );
    scene.push_back( keypoints_scene[ good_matches[i].trainIdx ].pt );
    }

    Mat H = findHomography( obj, scene, CV_RANSAC );

    //-- Get the corners from the image_1 ( the object to be "detected" )
    std::vector<Point2f> obj_corners(4);
    obj_corners[0] = cvPoint(0,0); obj_corners[1] = cvPoint( img_object.cols, 0 );
    obj_corners[2] = cvPoint( img_object.cols, img_object.rows ); obj_corners[3] = cvPoint( 0, img_object.rows );
    std::vector<Point2f> scene_corners(4);

    perspectiveTransform( obj_corners, scene_corners, H);

    //-- Draw lines between the corners (the mapped object in the scene - image_2 )
    line( img_matches, scene_corners[0] + Point2f( img_object.cols, 0), scene_corners[1] + Point2f( img_object.cols, 0), Scalar(0, 255, 0), 4 );
    line( img_matches, scene_corners[1] + Point2f( img_object.cols, 0), scene_corners[2] + Point2f( img_object.cols, 0), Scalar( 0, 255, 0), 4 );
    line( img_matches, scene_corners[2] + Point2f( img_object.cols, 0), scene_corners[3] + Point2f( img_object.cols, 0), Scalar( 0, 255, 0), 4 );
    line( img_matches, scene_corners[3] + Point2f( img_object.cols, 0), scene_corners[0] + Point2f( img_object.cols, 0), Scalar( 0, 255, 0), 4 );

    //-- Show detected matches
    //imshow( "Good Matches & Object detection", img_matches );

    //waitKey(0);

    //if ((max_dist - min_dist) > FEATURE_MATCHING_THRESHOLD)
    if (min_dist < FEATURE_MATCHING_THRESHOLD)
        return img_matches;
    else
        return Mat();
}
